package crtl;

import java.util.Properties;


/**
 *
 * @author benjamin
 */
public class ProportionalFollowLeftWall2Ctrl extends ProportionalCtrl {

	private final int _minDSensorValue = 100;

	private final double[][] _controllerMatrix = {
		{ 0,  0,  0,  0, 0, 0.2,    2,  0.5},
		{ 0,  0,  0,  0, 0,   1, -1.7, -2.4}
	};
	//private final double _constants[] = {0, 0, 0, 0, 0, 0.50, 0, 0};
	private final double _constants[] = {0.065, 0.065};
	private boolean _finished = false;
	
	public ProportionalFollowLeftWall2Ctrl(Properties properties) {
		super();
		
		System.out.println("Proportional - Follow Left Wall 2 Controller initialized");
	}
	
	
	@Override
	protected boolean finished() {
		return _finished;
	}

	@Override
	protected double[] getConstants() {
		return _constants;
	}

	@Override
	protected double[][] getControllMatrix() {
		return _controllerMatrix;
	}

	@Override
	protected double[] fetchAndPrepareSensorValues() {
		double[] s = readDistanceSensors();
		s = calcAverage(s);
		double max = findMax(s);
		if (max < _minDSensorValue) {
			max = _minDSensorValue;
		}
		return normalize(s, _minDSensorValue, max);
	}
	
	
	
}
